﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Drifting.Graphics.EffectObjects;
using Drifting.MotionPlanning.Primitives;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Drifting.Foundation;
using Drifting.MotionPlanning.MultiThreading.Planners;

namespace Drifting.Graphics.Visualizers
{
    public class Skin_Point2D : Skin_G
    {
        Line lineBlue;
        Line lineRed;
        Box box;
        List<SimpleObs> env;
        SST sst;

        public Skin_Point2D(List<State> train, GraphicsDevice device, List<SimpleObs> Env)
            : base(train, device)
        {
            lineBlue = new Line(0.01f, Color.Blue, device);
            lineRed = new Line(0.04f, Color.Red, device);
            box = new Box(1, 1, 0.0001f, Color.LightBlue, device);
            env = Env;
        }

        public void SetPlanner(SST sst)
        {
            this.sst = sst;
        }

        override public bool TranslateStateForVisualization(State x, ref float[] q_tmp, ref Color colorOfCost, double costBound)
        {
            if (x == null) return false;
            double[] q = x.Q;
            if (q == null) return false;

            double cost = x.Cost;
            double normalizedCost = 1 - cost / costBound;
            double complementaryNormalizedColor = 1 - normalizedCost;

            Color lowColor = Color.Green;
            Color highColor = Color.Red;
            colorOfCost.A = 255;
            colorOfCost.R = (byte)(normalizedCost * lowColor.R + complementaryNormalizedColor * highColor.R);
            colorOfCost.G = (byte)(normalizedCost * lowColor.G + complementaryNormalizedColor * highColor.G);
            colorOfCost.B = (byte)(normalizedCost * lowColor.B + complementaryNormalizedColor * highColor.B);

            q_tmp[0] = (float)q[0];
            q_tmp[2] = (float)q[1];
            q_tmp[1] = 0;

            return true;
        }

        private bool GoalReached(double[] q)
        {
            double x = q[0];
            double y = q[1];

            if (x > 4.25 && x < 4.75)
                if (y > 4.25 && y < 4.75)
                    return true;

            return false;
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            for (int i = 0; i < env.Count; ++i)
            {
                SimpleRec obs = (SimpleRec)env[i];

                Matrix config =
                    Matrix.CreateScale((float)obs.ry * 2, 1, (float)obs.rx * 2) * 
                    Matrix.CreateTranslation((float)obs.y, 0, (float)obs.x);
                box.Draw(camera, projection, config);
            }

            if (train == null) return;
            lock (train)
            {
                float[] q1_tmp = new float[3];
                float[] q2_tmp = new float[3];
                Color colorOfCost = new Color();
                double costBound = 10;
                HashSet<Edge> closedSet = new HashSet<Edge>();

                for (int i = 0; i < train.Count; ++i)
                {
                    State x = train[i];
                    if (x == null) continue;
                    Edge parentE = x.ParentEdge;

                    while (parentE != null)
                    {
                        State x_parent = parentE.entry;

                        if (closedSet.Contains(parentE) == false)
                        {
                            TranslateStateForVisualization(x_parent, ref q1_tmp, ref colorOfCost, costBound);
                            TranslateStateForVisualization(x, ref q2_tmp, ref colorOfCost, costBound);

                            lineBlue.Draw(camera, projection, q1_tmp, q2_tmp);
                            lineBlue.Draw(camera, projection, q2_tmp, q1_tmp);


                            //skin_state.Draw(camera, projection, q2_tmp, colorOfCost);   //  drawing the states

                            closedSet.Add(parentE);
                        }
                        x = x_parent;
                        parentE = x_parent.ParentEdge;
                    }
                }

                
                if (sst != null && sst.x_solution != null)
                {
                    State x = sst.x_solution;
                    Edge parentE = x.ParentEdge;
                    while (parentE != null)
                    {
                        State x_parent = parentE.entry;

                        TranslateStateForVisualization(x_parent, ref q1_tmp, ref colorOfCost, costBound);
                        TranslateStateForVisualization(x, ref q2_tmp, ref colorOfCost, costBound);

                        lineRed.Draw(camera, projection, q1_tmp, q2_tmp);
                        lineRed.Draw(camera, projection, q2_tmp, q1_tmp);

                        x = x_parent;
                        parentE = x_parent.ParentEdge;
                    }

                }
                


            }

        }
    }
}
